import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;

public class Main {
    public static void main(String[] args) {
    	// Create Balancer
    	Balancer balancer = new Balancer(Motor.B, Motor.C, SensorPort.S2, new PID(1000, 0, 0));
    	// Calibrate Light Sensor Infinitely
    	// System.out.println("Press to calibrate...");
    	// Button.waitForPress();
    	balancer.Calibrate();
    	// Or just use a constant
    	//balancer.SetLightValue(140);
    	for (int i = 0; i <= 1; i++)
    		for (int j = 0; j <= 1; j++)
    			for (int k = 0; k <= 1; k++) {
    		    	LCD.clearDisplay();
    		    	System.out.println("Press B2 [" + i + "," + j + "," + k + "]");
    		    	balancer.SetPID(new PID(i * 1000, j * 1000, k * 1000));
    		    	while (!Button.BUTTONS[3].isPressed());
    		    	// While Not ButtonPress
    		    	while (!Button.BUTTONS[0].isPressed())
    		    		balancer.Update();
    		    	balancer.Stop();
    			}
    }
}
